#include "Ping.h"
#include "Messages.pb.h"
#include <unistd.h>
#include <sys/time.h>
#include <algorithm>

Ping::Ping():pub_(),sub_(),reader_(*this),sendTime_(),ts_()
{
  ts_.reserve(250000);
  {
    fsk::msg::Message02 m;
    sub_.registerHandler(m,reader_);
  }
  fsk::msg::Message01 msg;
  msg.set_val01(0);
  sendTime_ = systemTime();
  ::usleep(5000);
  pub_.publish(msg);
}

Ping::~Ping()
{
  fsk::msg::Message02 m;
  sub_.unregisterHandler(m,reader_);
  
  double sum=0.0;
  for (TimeStampList::iterator iter = ts_.begin(); iter != ts_.end(); ++iter)
  {
    sum += *iter;
    printf("(%s:%d) %lf\n",__FILE__,__LINE__,*iter);
  }
  printf("(%s:%d) min: %lf\n",__FILE__,__LINE__,*std::min_element(ts_.begin(), ts_.end()));
  printf("(%s:%d) max: %lf\n",__FILE__,__LINE__,*std::max_element(ts_.begin(), ts_.end()));
  printf("(%s:%d) mean: %lf\n",__FILE__,__LINE__,sum/ts_.size());
  printf("(%s:%d) elements: %lu\n",__FILE__,__LINE__,ts_.size());
}

Ping::MsgReader::MsgReader(Ping& obj):obj_(obj)
{
}

void Ping::MsgReader::handle(fsk::msg::Message02& msg)
{
  //printf("(%s:%d) handling msg %d\n",__FILE__,__LINE__,msg.val01());
  msg.set_val01(msg.val01()+1);
  obj_.sendResponse(msg);
}

void Ping::sendResponse(fsk::msg::Message02& msg)
{
  const double elapsed=systemTime() - sendTime_;
  ts_.push_back(elapsed);
  fsk::msg::Message01 response;
  response.set_val01(msg.val01());
  sendTime_ = systemTime();
  pub_.publish(response);
}

double Ping::systemTime() const
{
  struct timeval currentTime;
  gettimeofday(&currentTime, NULL);
  const double val = currentTime.tv_sec + double(currentTime.tv_usec)/1000000.0;
  return val;
}
